/**
 * @file pid_controller.c
 * @brief PID Controller implementation
 * @author Flight Control Team
 * @date 2025-10-31
 */

#include "pid_controller.h"
#include <string.h>

/**
 * @brief Initialize PID controller
 */
void PID_Init(PID_Controller_t *pid, float kp, float ki, float kd, float dt)
{
    if (pid == NULL) {
        return;
    }
    
    memset(pid, 0, sizeof(PID_Controller_t));
    
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->dt = dt;
    pid->output_min = -1.0f;
    pid->output_max = 1.0f;
    pid->integral_min = -1.0f;
    pid->integral_max = 1.0f;
    pid->enable_anti_windup = true;
}

/**
 * @brief Set PID gains
 */
void PID_SetGains(PID_Controller_t *pid, float kp, float ki, float kd)
{
    if (pid == NULL) {
        return;
    }
    
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
}

/**
 * @brief Set output limits
 */
void PID_SetOutputLimits(PID_Controller_t *pid, float min, float max)
{
    if (pid == NULL) {
        return;
    }
    
    pid->output_min = min;
    pid->output_max = max;
}

/**
 * @brief Set integral limits
 */
void PID_SetIntegralLimits(PID_Controller_t *pid, float min, float max)
{
    if (pid == NULL) {
        return;
    }
    
    pid->integral_min = min;
    pid->integral_max = max;
}

/**
 * @brief Update PID controller
 */
float PID_Update(PID_Controller_t *pid, float setpoint, float measurement)
{
    if (pid == NULL) {
        return 0.0f;
    }
    
    /* Calculate error */
    float error = setpoint - measurement;
    
    /* Proportional term */
    float p_term = pid->kp * error;
    
    /* Integral term */
    pid->integral += error * pid->dt;
    
    /* Clamp integral */
    if (pid->integral > pid->integral_max) {
        pid->integral = pid->integral_max;
    } else if (pid->integral < pid->integral_min) {
        pid->integral = pid->integral_min;
    }
    
    float i_term = pid->ki * pid->integral;
    
    /* Derivative term */
    float derivative = (error - pid->prev_error) / pid->dt;
    float d_term = pid->kd * derivative;
    
    /* Calculate output */
    float output = p_term + i_term + d_term;
    
    /* Clamp output */
    if (output > pid->output_max) {
        output = pid->output_max;
        
        /* Anti-windup: stop integral accumulation */
        if (pid->enable_anti_windup && error > 0) {
            pid->integral -= error * pid->dt;
        }
    } else if (output < pid->output_min) {
        output = pid->output_min;
        
        /* Anti-windup: stop integral accumulation */
        if (pid->enable_anti_windup && error < 0) {
            pid->integral -= error * pid->dt;
        }
    }
    
    /* Save error for next iteration */
    pid->prev_error = error;
    
    return output;
}

/**
 * @brief Reset PID controller
 */
void PID_Reset(PID_Controller_t *pid)
{
    if (pid == NULL) {
        return;
    }
    
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
}

